#include <iostream>
#include <fstream>
#include <iterator>
#include <string>
#include <vector>

#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <dirent.h>

#include <signal.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>

#include <ros/ros.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>

#include <point_cloud/MapManager.hpp>

std::string input_topic, output_folder;
PointCloud::Ptr cloudIn(new PointCloud());
int frame_id = 0, begin_id, end_id, segment_size;
int b_idx, e_idx; // 用来为分段地图命名
MapManager map;
double resolution;

void saveMap()
{
    std::stringstream ss;
    ss << output_folder << "/" << b_idx << "_" << e_idx << ".pcd";

    ROS_WARN_STREAM("Merge " << e_idx-b_idx+1 << " frames from topic:" << input_topic);
    ROS_WARN_STREAM("Result save to " << ss.str());

    pcl::io::savePCDFileBinaryCompressed(ss.str(), *map.getMapPtr());
}

void sigintHandler(int sig) // 收到ctrl+c时运行该函数
{
    saveMap();
	ros::shutdown();
}

void cloudHandler(const sensor_msgs::PointCloud2ConstPtr &cloudMsg)
{
    cloudIn->clear();    
    pcl::fromROSMsg(*cloudMsg, *cloudIn);
    std::string op;
    if(frame_id < begin_id) op = "Skip";
    else{
        op = "Add";
        map.addFrame(cloudIn);

        if(segment_size>0 && frame_id>0){ // 分段存储
            if((frame_id-begin_id+1) % segment_size == 0){
                saveMap();
                b_idx = e_idx+1; // 更新下一段的编号
                map.setNewMap(cloudIn);
            }
        }
    }
    
    ROS_INFO_STREAM(op + " frame " << frame_id++ << " size:" << cloudIn->points.size());
    
    e_idx = frame_id;

    if(end_id != -1 && frame_id > end_id)
        sigintHandler(SIGINT);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "bag2map");
    ros::NodeHandle n("~");
    
    n.getParam("input_topic", input_topic);
    n.getParam("output_folder", output_folder);
    n.param<double>("resolution", resolution, 0.03);
    n.param<int>("begin_id", begin_id, 0);
    n.param<int>("end_id", end_id, -1);
    n.param<int>("segment_size", segment_size, 300);

    ROS_WARN_STREAM("parameters:");
    ROS_INFO_STREAM("cloud_topic = " << input_topic);
    ROS_INFO_STREAM("begin_id = " << begin_id);
    ROS_INFO_STREAM("end_id = " << end_id);
    ROS_INFO_STREAM("segment_size = " << segment_size);
    ROS_INFO_STREAM("resolution = " << resolution);

    b_idx = begin_id; // 记录分段存地图的起始编号

    std::stringstream ss;
    ss << output_folder << "/bag2map_output_" << begin_id << "_" << end_id;
    ss >> output_folder;

    if (access(output_folder.c_str(), 0) == -1) // 测试不存在
        if(mkdir(output_folder.c_str(), 0777) != 0) // 创建成功会返回0
            std::cout << "Make " + output_folder + " error!" << std::endl;

    map.init(resolution);

    signal(SIGINT, sigintHandler);

    ros::Subscriber sub = n.subscribe<sensor_msgs::PointCloud2>(input_topic, 100, cloudHandler);
    ros::spin();

    return 0;
}